%% Apollo Powered Descent Guidance
clear all;
clc;

%% 参数初始化
% 天体参数
miu = 4.88775e+12;  %月球引力常数(m^3/s^2)
RLunar = 1738*1000; %月球半径(m)
% 推力器参数
F = 1500;           %常力推力器推力(N)
Isp = 300;          %推力器比冲(s)
gE = 9.8;           %地表重力加速度(m/s^2)
C = Isp*gE;
% 初始条件
r = 1753*1000;      %m
alpha = 5/180*pi;   %弧度
beta = (1e-6)/180*pi;%弧度
u = 0;              %m/s
v = 1692;           %m/s
w = 0;              %m/s
m = 600;            %kg
psi = pi/2;
phi = -pi;
aF = F/m;
aH = aF*sin(psi);
% 终端条件
rFinal = 1740*1000; %终端月心距(m)
uFinal = 0;
vFinal = 0;
wFinal = 0;
% 设定ODE45递推参数
dt = 0.1;
Dt = 0;
option = odeset('RelTol', 1e-7);
tspan = [0 dt];
% 调节项
tFinal = 600;     %动力下降段总时间(s)
gainR = 12.0;

%% 画图所需数据
num = tFinal/dt;
position = zeros(3, num);
altitude = zeros(1, num);
distance = zeros(1, num);
time = zeros(1, num);

%% 制导过程
cnt = 0;
Xk = [r; alpha; beta; u; v; w; m];
while 1
    % 动力学推导
    [tOrbit, xOrbit] = ode45(@(t,x) DescentDynamic(t, x, miu, C, F, psi, phi), tspan, Xk, option);
    Xk = xOrbit(end,:)';
    r = Xk(1);
    alpha = Xk(2);
    beta = Xk(3);
    u = Xk(4);
    v = Xk(5);
    w = Xk(6);
    m = Xk(7);
    aF = F/m;
    aH = aF*sin(psi);
    
    % 控制律
    %计算剩余时间
    tgo = tFinal - Dt;
    if cnt == 0
        tgo0 = tgo;
    end
    %求径向加速度
    kr = gainR;
    ax0 = 2*(1-kr/3)*(uFinal-u)/tgo + kr*(rFinal-r-u*tgo)/tgo^2;
    %求推力控制角
    psi = acos( (ax0 + miu/r^2 - (v^2+w^2)/r) /aF );
    phi = acos((vFinal-v) / sqrt((wFinal-w)^2+(vFinal-v)^2) );
    
    cnt = cnt + 1;
    Dt = Dt + dt;
    
    % 计算绘图所需量
    time(1, cnt) = Dt;
    altitude(1, cnt) = r - RLunar;
    position(1, cnt) = r*cos(beta)*cos(alpha);
    position(2, cnt) = r*cos(beta)*sin(alpha);
    position(3, cnt) = r*sin(beta);
    
    if tgo < 1e-1
        break;
    end
end

%% 画图
for i = 1:1:num
    position(:, i) = position(:, i) - position(:, num);
    distance(i) = norm(position(:,i));
end

figure;
plot(distance, altitude);
xlabel('distance(m)');
ylabel('altitude(m)');

%% 导出文件
%csvwrite('kr12.csv', [distance; altitude]);

%% 动力学模型
% 此为月球软着陆的动力学模型
% 涉及坐标系为月心惯性坐标系、探测器轨道坐标系、探测器本体系
% 探测器在月心惯性系的位置为(r,alpha,beta)，分别为月心距、经度、纬度
% psi和phi为轨道坐标系中的推力方向角
% F为常推力发动机推力大小
% C = Isp * gE, Isp为发动机比冲, gE为地表重力加速度
% r根据u更新
% alpha,beta根据w,v更新
% u,v,w为探测器轨道坐标系下x0,y0,z0轴的速度分量，x0指向径向,y0指向运动正向
% m根据F更新
function X = DescentDynamic(t, x, miu, C, F, psi, phi)

r = x(1);
alpha = x(2);
beta = x(3);
u = x(4);
v = x(5);
w = x(6);
m = x(7);

dr = u;
dalpha = w/(r*sin(beta));
dbeta = v/r;
du = F*cos(psi)/m - miu/r^2 + (v^2+w^2)/r;
dv = F*sin(psi)*cos(phi)/m - u*v/r + w^2/(r*tan(beta));
dw = F*sin(psi)*sin(phi)/m - u*w/r - v*w/(r*tan(beta));
dm = -F/C;
X = [dr; dalpha; dbeta; du; dv; dw; dm];

end